﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MonoPhysics
{
    enum ObjectType { Moveable = 1, Fixed = 2, Actionable = 4 };

    class ContactGraph
    {
        List<Node> nodes = new List<Node>();
        List<Edge> edges = new List<Edge>();
        List<Edge> N = new List<Edge>();

        public void ReinitDamagedNodes()
        {
            foreach (Edge e in this.edges)
            {/*
                RigidBody r = (e.ue1.M == 0) ? e.ue2 : e.ue1;
                foreach (Vector v in e.PenetraDepth)
                {
                    r.Pos += v;
                }
                e.PenetraDepth.Clear();//*/
                foreach (CircularLinkedListNode<PointD, RigidBody> n in e.NodesToReinit)
                {
                    n.nodes[0].fils = n.nodes[1].fils;    // on repointe sur les nodes initiaux (pointeurs noirs)
                    n.nodes[0].parent = n.nodes[1].parent;
                }
                e.NodesToReinit.Clear();
            }
        }


        public void ReplaceObjects()
        {
            foreach (Edge e in this.edges)
            {
                RigidBody r;
                if (e.ue1.M == 0)
                {
                    if (e.ue2.M == 0)
                    {
                        if (e.ue1.PhysicStats.V.isNull())
                        {
                            if (e.ue2.PhysicStats.V.isNull())
                            {
                                e.PenetraDepth.Clear();
                                continue;
                            }
                            else
                                r = e.ue2;
                        }
                        else
                            r = e.ue1;
                    }
                    else
                        r = e.ue2;
                }
                else
                    r = e.ue1;
                for (int i = 0; i < e.PenetraDepth.Count; i++) //each (Vector v in e.PenetraDepth)
                {
                    //if (e.PenetraDepth.Count > 0)
                    //{
                    Vector v = e.PenetraDepth[i];
                    if (float.IsNaN(v.X) || float.IsNaN(v.Y) || (v.X * v.X + v.Y * v.Y > 400))
                    {
                        //Console.WriteLine("NAN");
                        continue;
                    }
                    Vector correct = v;
                    if (r.LockX)
                        correct.X = 0;
                    if (r.LockY)
                        correct.Y = 0;
                    r.Pos += correct;
                    /*
                    for (int j = i + 1; j < e.PenetraDepth.Count; j++)
                        e.PenetraDepth[j] -= v;//*/
                }
                //*/

                e.PenetraDepth.Clear();
            }
        }
        public void AddEdges(List<Edge> es)
        {

            // Ajout des liens et marquage des liens en rouge.
            foreach (Edge e in es)
            {
                //if (e.ue1.M == 0 && e.ue2.M == 0)
                //    continue;
                bool found = false;
                foreach (Edge ed in this.edges)
                {
                    if ((ed.ue2 == e.ue2 && ed.ue1 == e.ue1) || (ed.ue2 == e.ue1 && ed.ue1 == e.ue2))
                    {
                        found = true;
                        ed.color = Edge.Color.Red;
                        if (ed.type == Edge.EdgeType.CloseProximity)
                            ed.type = Edge.EdgeType.PersistantCloseProximity;
                        else if (ed.type == Edge.EdgeType.VanishedOnceCloseProximity || ed.type == Edge.EdgeType.VanishedOnceContact)
                            ed.type = Edge.EdgeType.CloseProximity;

                        break;
                    }
                }
                if (!found)
                {
                    e.ue1.Sleeping = false;
                    e.ue2.Sleeping = false;
                    e.color = Edge.Color.Red;
                    e.type = Edge.EdgeType.CloseProximity;
                    this.AddEdge(e);
                    e.ue1.Edges.Add(e);
                    e.ue2.Edges.Add(e);
                }
            }

            // Supression des liens gris
            int l = this.edges.Count;
            for (int i = 0; i < l; )
            {
                Edge e = this.edges[i];
                if (e.color != Edge.Color.Red)
                {
                    if (e.type == Edge.EdgeType.Vanished || e.type == Edge.EdgeType.VanishedOnceCloseProximity || e.type == Edge.EdgeType.VanishedOnceContact)
                    {
                        //if (Obsolete(e.ue1, e.ue2))
                        //{
                        e.ue1.Sleeping = false;
                        e.ue2.Sleeping = false;
                        this.RemoveEdge(e);
                        l--;
                        continue;
                        //}
                        //else
                        //    e.type = Edge.EdgeType.Vanished;
                    }
                    else
                    {
                        if (e.type == Edge.EdgeType.PersistantContact)
                            e.type = Edge.EdgeType.VanishedOnceContact;
                        else
                            e.type = Edge.EdgeType.VanishedOnceCloseProximity;
                    }
                }
                e.color = Edge.Color.Grey;
                i++;
            }
        }

        private void BuildEdgeList()
        {
            this.N.Clear();
            foreach (Edge e in this.edges)
            {
                if (!NarrowPhase.Rule(e) || e.ue1.logical || e.ue2.logical || e.ue1.Sleeping || e.ue2.Sleeping)
                    continue;
                if ((e.xForm == e.ue1.Pos - e.ue2.Pos) || (e.xForm == e.ue2.Pos - e.ue1.Pos))
                {
                    if (e.type == Edge.EdgeType.Contact)
                        e.type = Edge.EdgeType.PersistantContact;
                    continue;
                }
                this.N.Add(e);
            }
        }

        public void ApplyNarrowPhase(MonoPhysics engine)
        {
            BuildEdgeList();
            foreach (Edge e in this.N)
            {
                bool persist = NarrowPhase.GetContacts(engine, e);
                Edge.EdgeType eold = e.type;
                if (eold == Edge.EdgeType.Contact || eold == Edge.EdgeType.PersistantContact)
                {
                    if (persist)
                        e.type = Edge.EdgeType.PersistantContact;
                    else
                        e.type = Edge.EdgeType.VanishedOnceContact;
                }
                else
                    if (e.collisions.Count != 0)
                        e.type = Edge.EdgeType.Contact;
            }

        }

        public List<ContactGraph> BuildContactGroups()
        {
            List<ContactGraph> res = new List<ContactGraph>();
            foreach (Edge e in this.N)
                e.color = Edge.Color.White;
            foreach (Edge e in this.N)
            {
                if (e.color == Edge.Color.White)
                {
                    ContactGraph G = new ContactGraph();
                    TraverseGroup(e, G);
                    res.Add(G);
                }
            }
            return res;
        }

        private void TraverseGroup(Edge e, ContactGraph G)
        {
            e.color = Edge.Color.Grey;
            G.AddEdge(e);
            if (e.ue1.M != 0)
                foreach (Edge w in e.ue1.Edges)
                    if (w.color == Edge.Color.White)
                        TraverseGroup(w, G);
            if (e.ue2.M != 0)
                foreach (Edge w in e.ue2.Edges)
                    if (w.color == Edge.Color.White)
                        TraverseGroup(w, G);

            //e.color = Edge.Color.Black;
        }

        #region Creation/Ajout/Suppretion
        private void RemoveEdge(Edge e)
        {
            this.edges.Remove(e);
            e.ue1.Edges.Remove(e);
            e.ue2.Edges.Remove(e);
            if (e.ue1.Edges.Count == 0)
                this.nodes.Remove(e.ue1);
            if (e.ue2.Edges.Count == 0)
                this.nodes.Remove(e.ue2);
        }
        private void AddEdge(Edge e)
        {
            this.edges.Add(e);
            if (!this.nodes.Contains(e.ue1))
                this.nodes.Add(e.ue1);
            if (!this.nodes.Contains(e.ue2))
                this.nodes.Add(e.ue2);
        }
        public void DeleteNode(RigidBody n)
        {
            this.edges.RemoveAll(
                delegate(Edge e)
                {
                    if (e.ue1 == n || e.ue2 == n)
                    {
                        e.ue1.Sleeping = false;
                        e.ue2.Sleeping = false;
                        return true;
                    }
                    else
                        return false;
                }
            );
            this.nodes.Remove(n);
        }
        #endregion

        #region Predicats
        private bool Obsolete(RigidBody r1, RigidBody r2)
        {
            return true;//(Math.Abs(r1.Pos.X - r2.Pos.X) > r1.Shape.BoundaryRect.Width + r2.Shape.BoundaryRect.Width
            //|| (Math.Abs(r1.Pos.Y - r2.Pos.Y) > r1.Shape.BoundaryRect.Height + r2.Shape.BoundaryRect.Height));
        }
        #endregion

        #region Executions
        public void GenerateEvents()
        {
            foreach (Edge e in this.edges)
            {
                PhysicEvent pe = null;
                switch (e.type)
                {
                    case Edge.EdgeType.CloseProximity:
                        ////Console.WriteLine("Close proximity");
                        if (e.ue1.HasInteractiveModeEnabled)
                            pe += e.ue1.CloseProximityEvent;
                        if (e.ue2.HasInteractiveModeEnabled)
                            pe += e.ue2.CloseProximityEvent;
                        break;
                    case Edge.EdgeType.Contact:
                        //Console.WriteLine("Contact");
                        if (e.ue1.HasInteractiveModeEnabled)
                            pe += e.ue1.NewContactEvent;
                        if (e.ue2.HasInteractiveModeEnabled)
                            pe += e.ue2.NewContactEvent;
                        break;
                    case Edge.EdgeType.PersistantCloseProximity:
                        ////Console.WriteLine("Persistant close proximity");
                        if (e.ue1.HasInteractiveModeEnabled)
                            pe += e.ue1.PersistantCloseProximityEvent;
                        if (e.ue2.HasInteractiveModeEnabled)
                            pe += e.ue2.PersistantCloseProximityEvent;
                        break;
                    case Edge.EdgeType.PersistantContact:
                        //Console.WriteLine("Persistant contact");
                        if (e.ue1.HasInteractiveModeEnabled)
                            pe += e.ue1.PersistantContactEvent;
                        if (e.ue2.HasInteractiveModeEnabled)
                            pe += e.ue2.PersistantContactEvent;
                        break;
                    case Edge.EdgeType.VanishedOnceCloseProximity:
                        ////Console.WriteLine("Vanished once close proximity");
                        if (e.ue1.HasInteractiveModeEnabled)
                            pe += e.ue1.VanishedCloseProximityEvent;
                        if (e.ue2.HasInteractiveModeEnabled)
                            pe += e.ue2.VanishedCloseProximityEvent;
                        break;
                    case Edge.EdgeType.VanishedOnceContact:
                        //Console.WriteLine("Vanished once contact");
                        if (e.ue1.HasInteractiveModeEnabled)
                            pe += e.ue1.VanishedContactEvent;
                        if (e.ue2.HasInteractiveModeEnabled)
                            pe += e.ue2.VanishedContactEvent;
                        break;
                }
                if (pe != null)
                    pe(e);
            }
        }
        public void Solve(float dt)
        {
            foreach (Edge e in this.edges)
                e.Solve(dt);
        }
        #endregion
    }


    public class Node
    {
        List<Edge> edges = new List<Edge>();
        [System.ComponentModel.Browsable(false)]
        public List<Edge> Edges
        { get { return edges; } }
    }


    public class Edge
    {
        public enum Color { Red, Grey, White, Black }
        public enum EdgeType { NoMark, Contact, PersistantContact, CloseProximity, PersistantCloseProximity, Vanished, VanishedOnceCloseProximity, VanishedOnceContact }
        public Vector penNormal;
        public List<Collision> collisions;
        public RigidBody ue1;
        public RigidBody ue2;
        public RigidBody impulse_cible;
        public RigidBody impulse_autre;

        public bool hasPenatrate;
        public int PenetrationPointIndex;
        public Color color;
        public EdgeType type;
        public Vector xForm;
        public bool isCollidingX;
        public bool isCollidingY;
        public List<Vector> PenetraDepth = new List<Vector>();
        public List<CircularLinkedListNode<PointD, RigidBody>> NodesToReinit = new List<CircularLinkedListNode<PointD, RigidBody>>();
        public void Solve(float dt)
        {
            ImpulseSolver.ApplyImpulse(this);
        }
    }

    public struct SphereCollision
    {
        public readonly Circle c1;
        public readonly Circle c2;

        public SphereCollision(Circle c1, Circle c2)
        {
            this.c1 = c1;
            this.c2 = c2;
        }
    }
}
